/*
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
 *
 *  Authors:
 *  	Albert Marieges Garcia
 *  	Álex Pardo Fernández
 *  	Enric Mayas Márquez
 *  	Juan Carlos Calvo Tejedor
 *  	Marc Gironés Dezsényi
 *  
 *  Contact:
 *  	bcn.tours.android@gmail.com
 *  Webpage:
 *  	bcntours.co.cc
 *
 */

package com.BCNTours.AR.Utils;

/**
 * Support class used at obtaining coordinates and data related to those, such at distances.
 */

public class Coordinates{
	
	private float lt = 0;
	private float ln = 0;
	private static final float LATITUDE_DEGREE = 30.82f;
	/**
	 * Device distance in meters
	 */
	public static final float DEV_DISTANCE = 0.3f;
	/**
	 * Earth's equatorial radius
	 */
	private static final float a = 6378137; 
	
	/**
	 * Earth's polar radius
	 */
	private static final float b = 6356752.3f;
	
	/**
	 * Constructor de la clase
	 * 
	 * @param latitude
	 * @param longitude
	 */
	public Coordinates(float latitude, float longitude){
		lt = latitude;
		ln = longitude;
	}
	
	public float getLt() {
		return lt;
	}
	public void setLt(float lt) {
		this.lt = lt;
	}
	public float getLn() {
		return ln;
	}
	public void setLn(float ln) {
		this.ln = ln;
	}
	
	
	

	/**
	 * Calculate the distance in kilometer between two coordinate.
	 * @param c
	 * @return
	 */
	public float getDistance_km(Coordinates c){
		int MAXITERS = 20;
//		float[] result = new float[1];
		float lt2 = c.getLt();
		float ln2 = c.getLn();
		float ltTmp = lt;
		float lnTmp = ln;
		
        // Convert lat/long to radians
        ltTmp *= Math.PI / 180.0;
        lt2 *= Math.PI / 180.0;
        lnTmp *= Math.PI / 180.0;
        ln2 *= Math.PI / 180.0;

        double a = 6378137.0; // WGS84 major axis
        double b = 6356752.3142; // WGS84 semi-major axis
        double f = (a - b) / a;
        double aSqMinusBSqOverBSq = (a * a - b * b) / (b * b);

        double L = ln2 - lnTmp;
        double A = 0.0;
        double U1 = Math.atan((1.0 - f) * Math.tan(ltTmp));
        double U2 = Math.atan((1.0 - f) * Math.tan(lt2));

        double cosU1 = Math.cos(U1);
        double cosU2 = Math.cos(U2);
        double sinU1 = Math.sin(U1);
        double sinU2 = Math.sin(U2);
        double cosU1cosU2 = cosU1 * cosU2;
        double sinU1sinU2 = sinU1 * sinU2;

        double sigma = 0.0;
        double deltaSigma = 0.0;
        double cosSqAlpha = 0.0;
        double cos2SM = 0.0;
        double cosSigma = 0.0;
        double sinSigma = 0.0;
        double cosLambda = 0.0;
        double sinLambda = 0.0;

        double lambda = L; // initial guess
        for (int iter = 0; iter < MAXITERS; iter++) {
            double lambdaOrig = lambda;
            cosLambda = Math.cos(lambda);
            sinLambda = Math.sin(lambda);
            double t1 = cosU2 * sinLambda;
            double t2 = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda;
            double sinSqSigma = t1 * t1 + t2 * t2; // (14)
            sinSigma = Math.sqrt(sinSqSigma);
            cosSigma = sinU1sinU2 + cosU1cosU2 * cosLambda; // (15)
            sigma = Math.atan2(sinSigma, cosSigma); // (16)
            double sinAlpha = (sinSigma == 0) ? 0.0 :
                cosU1cosU2 * sinLambda / sinSigma; // (17)
            cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
            cos2SM = (cosSqAlpha == 0) ? 0.0 :
                cosSigma - 2.0 * sinU1sinU2 / cosSqAlpha; // (18)

            double uSquared = cosSqAlpha * aSqMinusBSqOverBSq; // defn
            A = 1 + (uSquared / 16384.0) * // (3)
                (4096.0 + uSquared *
                 (-768 + uSquared * (320.0 - 175.0 * uSquared)));
            double B = (uSquared / 1024.0) * // (4)
                (256.0 + uSquared *
                 (-128.0 + uSquared * (74.0 - 47.0 * uSquared)));
            double C = (f / 16.0) *
                cosSqAlpha *
                (4.0 + f * (4.0 - 3.0 * cosSqAlpha)); // (10)
            double cos2SMSq = cos2SM * cos2SM;
            deltaSigma = B * sinSigma * // (6)
                (cos2SM + (B / 4.0) *
                 (cosSigma * (-1.0 + 2.0 * cos2SMSq) -
                  (B / 6.0) * cos2SM *
                  (-3.0 + 4.0 * sinSigma * sinSigma) *
                  (-3.0 + 4.0 * cos2SMSq)));

            lambda = L +
                (1.0 - C) * f * sinAlpha *
                (sigma + C * sinSigma *
                 (cos2SM + C * cosSigma *
                  (-1.0 + 2.0 * cos2SM * cos2SM))); // (11)

            double delta = (lambda - lambdaOrig) / lambda;
            if (Math.abs(delta) < 1.0e-12) {
                break;
            }
        }

        float distance = (float) (b * A * (sigma - deltaSigma));
        return distance/1000;
    }

	/**
	 * Calculate the distance in mile between two coordinate
	 * @param c
	 * @return
	 */
	public float getDistance_mile(Coordinates c){
		return getDistance_km(c) * 0.62f;
	}
	
	/**
	 * Calculate the distance in yard between two coordinate
	 * @param c
	 * @return
	 */
	public float getDistance_yard(Coordinates c){
		return getDistance_yard(c) * 1093.6133f;
	}
    
	
	
	
	/**
	 * Calculates the angle between the camera and the resource
	 * @param c resource coordinates
	 * @param compassAzimuth azimuth from the phone
	 * @return
	 */
	public float getAngleToCam(Coordinates c, float compassAzimuth){
		
		float[] source = new float[2];
		float[] resource = new float[2];
		source[0] = (float) lt;
		source[1] = (float) ln;
		resource[0] = (float) c.getLt();
		resource[1] = (float) c.getLn();
		float res_azimuth = calculateResourceAzimuth(source, resource);
		float azimuth_user = calculateAzimuthFromUser(compassAzimuth, res_azimuth);
		return calculateAngleToCam(source, resource, compassAzimuth, azimuth_user, DEV_DISTANCE);
		
		
	}
	
	public float getAzimuth(){
		return (float) Math.atan(ln/lt);
		
	}
	
	
	/*
	 *
	 *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
	 *
	 *  This program is free software: you can redistribute it and/or modify
	 *  it under the terms of the GNU General Public License as published by
	 *  the Free Software Foundation, either version 3 of the License, or
	 *  (at your option) any later version.
	 *
	 *  This program is distributed in the hope that it will be useful,
	 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
	 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	 *  GNU General Public License for more details.
	 *
	 *  You should have received a copy of the GNU General Public License
	 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
	 *
	 *  Author : Raul Roman Lopez <rroman@gsyc.es>
	 *
	 */
	public float azimuthToResource(Coordinates resCoord){
		float azimuth,source [] = new float[2], resource[] = new float[2];
		
		source[0] = lt;
		source[1] = ln;
		
		resource[0] = resCoord.getLt();
		resource[1] = resCoord.getLn();
		
		azimuth = calculateResourceAzimuth(source, resource);
		
		return azimuth;
	}
	
	/*
	 *
	 *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
	 *
	 *  This program is free software: you can redistribute it and/or modify
	 *  it under the terms of the GNU General Public License as published by
	 *  the Free Software Foundation, either version 3 of the License, or
	 *  (at your option) any later version.
	 *
	 *  This program is distributed in the hope that it will be useful,
	 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
	 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	 *  GNU General Public License for more details.
	 *
	 *  You should have received a copy of the GNU General Public License
	 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
	 *
	 *  Author : Raul Roman Lopez <rroman@gsyc.es>
	 *
	 */
	/**
	 * ----- From ARviewer ------
	 * 
	 * This function calculates the azimuth of a resource from the user's position
	 * @param source is the user's (latitude, longitude)
	 * @param resource is the resource's (latitude, longitude)
	 * 
	 * @return This returns the azimuth in degrees
	 */
	public static float calculateResourceAzimuth(float[] source, float[] resource){
		float azimuth, dist_lat, dist_lng;
		
		float lat_degree = LATITUDE_DEGREE * 3600;
		float long_degree = (float) ((Math.PI/180)*Math.cos(Math.toRadians(source[0])) *
				Math.sqrt((Math.pow(a, 4)*Math.pow(Math.cos(Math.toRadians(source[0])),2) +
						Math.pow(b, 4)*Math.pow(Math.sin(Math.toRadians(source[0])),2))/
						(Math.pow(a, 2)*Math.pow(Math.cos(Math.toRadians(source[0])),2) +
								Math.pow(b, 2)*Math.pow(Math.sin(Math.toRadians(source[0])),2))
						));
		
		dist_lat = (resource[0] - source[0]) * lat_degree;
		dist_lng = (resource[1] - source[1]) * long_degree;
		
		azimuth = (float) Math.toDegrees(Math.atan2(dist_lng, dist_lat));
		
		if(azimuth<0)
			azimuth += 360;
		
		return azimuth;
	}
	
	
	
	
	/*
	 *
	 *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
	 *
	 *  This program is free software: you can redistribute it and/or modify
	 *  it under the terms of the GNU General Public License as published by
	 *  the Free Software Foundation, either version 3 of the License, or
	 *  (at your option) any later version.
	 *
	 *  This program is distributed in the hope that it will be useful,
	 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
	 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	 *  GNU General Public License for more details.
	 *
	 *  You should have received a copy of the GNU General Public License
	 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
	 *
	 *  Author : Raul Roman Lopez <rroman@gsyc.es>
	 *
	 */
	/**
	 * ----- From ARviewer ------
	 * 
	 * This function calculates the relative azimuth of the resource from the user
	 * @param user_azimuth is the azimuth of the user
	 * @param resource_azimuth is the azimuth of the resource
	 * @return The returned value is the angle in degrees
	 */
	public static float calculateAzimuthFromUser(float user_azimuth, float resource_azimuth){
		float azimuth = 500, azimuth_Aux;
		
		if(Math.abs(azimuth_Aux =  resource_azimuth - user_azimuth) < Math.abs(azimuth))
			azimuth = azimuth_Aux;
		
		if(Math.abs(azimuth_Aux =  (resource_azimuth - 360) - user_azimuth) < Math.abs(azimuth))
			azimuth = azimuth_Aux;
		
		if(Math.abs(azimuth_Aux =  (360 + resource_azimuth) - user_azimuth) < Math.abs(azimuth))
			azimuth = azimuth_Aux;
		
		
		return azimuth;
	}
	
	
	
	
	
	/*
	 *
	 *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
	 *
	 *  This program is free software: you can redistribute it and/or modify
	 *  it under the terms of the GNU General Public License as published by
	 *  the Free Software Foundation, either version 3 of the License, or
	 *  (at your option) any later version.
	 *
	 *  This program is distributed in the hope that it will be useful,
	 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
	 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	 *  GNU General Public License for more details.
	 *
	 *  You should have received a copy of the GNU General Public License
	 *  along with this program.  If not, see http://www.gnu.org/licenses/. 
	 *
	 *  Author : Raul Roman Lopez <rroman@gsyc.es>
	 *
	 */
	/**
	 * ----- From ARviewer ------
	 * 
	 * This function calculates the angle of an object regarding to the camera.
	 * @param p0_deg User's position (lat, long)
	 * @param p1_deg Object position (lat, long)
	 * @param or0 User's actual orientation
	 * @param or1 Object orientation regarding to the user
	 * @param camDistance Distance between camera and user
	 * @return The angle required or null if there's an error
	 */
	public static float calculateAngleToCam(float[] p0_deg, float[] p1_deg, float or0, float or1, float camDistance){
		float alpha;
		float lat_degree, long_degree;

		lat_degree = LATITUDE_DEGREE * 3600;
		long_degree =(float) ((Math.PI/180)*Math.cos(Math.toRadians(p0_deg[0])) *
				Math.sqrt((Math.pow(a, 4)*Math.pow(Math.cos(Math.toRadians(p0_deg[0])),2) +
						Math.pow(b, 4)*Math.pow(Math.sin(Math.toRadians(p0_deg[0])),2))/
						(Math.pow(a, 2)*Math.pow(Math.cos(Math.toRadians(p0_deg[0])),2) +
								Math.pow(b, 2)*Math.pow(Math.sin(Math.toRadians(p0_deg[0])),2))
						));
		
		//Cartesian system
		float[] p0 = {0, 0};
		float[] pCam = {(float) (camDistance * Math.sin(Math.toRadians(or0))), (float) (camDistance * Math.cos(Math.toRadians(or0)))};
		float[] p1 = { long_degree * (p1_deg[1] - p0_deg[1]), 
				lat_degree * (p1_deg[0] - p0_deg[0]) };
		
		float[] vector1 = { pCam[0] - p0[0], pCam[1] - p0[1] };
		float[] vector2 = { p1[0] - pCam[0], p1[1] - pCam[1] };
		
		float mod1 = (float) Math.sqrt(Math.pow(vector1[0], 2) + Math.pow(vector1[1], 2));
		float mod2 = (float) Math.sqrt(Math.pow(vector2[0], 2) + Math.pow(vector2[1], 2));;
		alpha = (float) Math.toDegrees(Math.acos((vector1[0] * vector2[0] + vector1[1] * vector2[1])/(mod1 * mod2)));
		
		if(or1 < 0)
			alpha = -alpha;
		
		return alpha;
	}
}